// Copyright (C) 2016 All rights reserved.
// Author: liufeng11@letv.com (Liu Feng)
// Description:
#include "HogFontFeatureExtractor.h"
#include <cstdio>
#include <cstdlib>
#include <cstring>

void HogFontFeatureExtractor::ExtractFeature(const unsigned char* data, std::vector<double>* feature) {
  if (!feature || !data) return;

  // Blur

  double centerWeight = 0.1592;
  double w1 = 0.0585;
  double w2 = 0.0965;
  double gray[ROWS*COLS] = { 0 };
  for (int i = 0; i < ROWS; ++i) {
    for (int j = 0; j < COLS; ++j) {
      gray[i*COLS+j] = centerWeight*P(data, i, j);
      gray[i*COLS+j] += w1*P(data, i-1, j-1);
      gray[i*COLS+j] += w2*P(data, i-1, j);
      gray[i*COLS+j] += w1*P(data, i-1, j+1);
      gray[i*COLS+j] += w2*P(data, i, j+1);
      gray[i*COLS+j] += w1*P(data, i+1, j+1);
      gray[i*COLS+j] += w2*P(data, i+1, j);
      gray[i*COLS+j] += w1*P(data, i+1, j-1);
      gray[i*COLS+j] += w2*P(data, i, j-1);
    }
  }

  // const unsigned char* gray = data;
  // Calcualate integral image
  double Ix[ROWS*COLS] = { 0 };
  double Iy[ROWS*COLS] = { 0 };

  for (int i = 0; i < ROWS; ++i) {
    for (int j = 0; j < COLS; ++j) {
      if (j == 0) {
        Ix[i*COLS] = P(gray, i, 1) - P(gray, i, 0);
      }
      else if (j == COLS-1) {
        Ix[i*COLS + j] = P(gray, i, j) - P(gray, i, j-1);
      }
      else {
        Ix[i*COLS + j] = P(gray, i, j + 1) - P(gray, i, j - 1);
      }
      
      if (i == 0) {
        Iy[j] = P(gray, 1, j) - P(gray, 0, j);
      }
      else if (i == ROWS-1) {
        Iy[i*COLS + j] = P(gray, i, j) - P(gray, i-1, j);
      }
      else {
        Iy[i*COLS + j] = P(gray, i+1, j) - P(gray, i-1, j);
      }
    }
  }

  ExtractHogFeature(Ix, Iy, ROWS, COLS, 8, feature);
  ExtractHogFeature(Ix, Iy, ROWS, COLS, 6, feature);
  ExtractHogFeature(Ix, Iy, ROWS, COLS, 4, feature);
  ExtractHogFeature(Ix, Iy, ROWS, COLS, 2, feature);
  ExtractHogFeature(Ix, Iy, ROWS, COLS, 1, feature);

}


void HogFontFeatureExtractor::ExtractHogFeature(const double* Ix, const double* Iy, int rows, int cols, int scale, std::vector<double>* v) {
  // ref: http://blog.csdn.net/q1007729991/article/details/53032776
  if (!v) return;
  static double bin[8] = {PI/8, 3*PI/8, 5*PI/8, 7*PI/8, 9*PI/8, 11*PI/8, 13*PI/8, 15*PI/8};
  double dx, dy;

  double *r = new double[scale*scale*8];

  if (!r) {
    printf("Memory allocated failed!\n");
    exit(-1);
  }

  memset(r, 0, scale*scale*8*sizeof(double));

  dx = (double)cols/scale;
  dy = (double)rows/scale; 
  for (int i = 0; i < rows; ++i ) {
    for (int j = 0; j < cols; ++j) {
      double x = P(Ix, i, j);
      double y = P(Iy, i, j);

      double f = sqrt(x*x+y*y);
      double theta = atan2(y, x);
      if (theta < 0) theta += 2*PI;
      
      int o = ((int)(theta*4/PI)) % 8;
      int o1, o2;

      double f1, f2;

      if (theta > bin[o]) {
        o1 = o;
        o2 = (o + 1) % 8;
        double l = (theta - bin[o1])*4/PI;
        f1 = (1-l)*f;
        f2 = l*f;
      }
      else {
        o2 = o;
        o1 = (o + 7) % 8;
        double r = (bin[o2]-theta)*4/PI;
        f1 = r*f;
        f2 = (1-r)*f;
      }


      int m = i/dy;
      int n = j/dx;
      
      r[o1*scale*scale + m*scale + n] += f1;
      r[o2*scale*scale + m*scale + n] += f2;
    }
  }
  
  for (int i = 0; i < scale*scale*8; ++i) {
    // v->push_back(r[i]/scale/scale);
    v->push_back(r[i]);
  }
  delete[] r;
}
